/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>

#include "MatchStorage.h"
#include "EnvironmentMeasurement2D.h"
#include "OccuGUI.h"
#include "BetBdtLocalizer.h"
#include "ownutils.h"
#include "CParticleFilter.h"

#define DEFAULT_J2B2_MEAS_FILE "../../../data/j2b2_measurement.txt"
#define DEFAULT_MAP_FILE "../../res/autmap-strip.txt"


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void printUsage(){
		fprintf(stderr,"\n--------------------------------------------------------------------\n");
		fprintf(stderr,"Scan Match\n");	
		fprintf(stderr,"\nThis application contains different use cases for BetB_dt Localizer\n");
		fprintf(stderr,"The different cases uses 'simulated' data which is requested as user input.\n");
		fprintf(stderr,"\nUsgage ./scanmatch [logfile]\n");
		fprintf(stderr," 	ScanMatch Demo. 	 [scan_meas] [env_out_file] [scan_meas_out]\n");
		fprintf(stderr,"--------------------------------------------------------------------\n\n\n");
}



void timer(bool isStart){
		static long int ss1,us1;
		long int ss2,us2,sec,us,ms;
		float fms;
		
		if(isStart) ownTime_GetTimeOfDay(&ss1,&us1);
		else{
			ownTime_GetTimeOfDay(&ss2,&us2);
			sec = ss2-ss1;
			us = us2-us1;
			ms = sec*1000+us/1000;
			fms=(float)sec*1000.0+(float)us/1000.0;
			fprintf(stderr,"Time elapsed (%.6fms = %.6fs) \n",fms,fms/1000.0);
		}
}

/// For reading J2B2 data from filet
int readScanLine181(FILE *f,Grid::TScan &meas, float &x,float &y,float &a){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&x,&y,&a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						//fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, mcl::pose &p ){
		float time_s;
		int i=0;
		char in_line[16096];
		char *ret;
		char c=0;
		
		ret = fgets (in_line, 16096, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return 0;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return 1;
				}
		}
		int cnt=0;
		while( (c!='\n')){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}


void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
/** 
* Test Very Local SLAM = Scan Match
* A "real-time" version of the B&B 
* Localizes on a small local map.
*
* @param *scan_meas odometry/scan input file name (currently J2B2)  
* @param *env_out_file CEnvironmentMeasurement2D output file
* @param *scan_meas_out outputs the scan/odo measurement file
* @param visu set true to use visualization
**/
void ScanMatchDemo(const char *scan_meas, const char *env_out_file, const char *scan_meas_out, bool visu){
	CEnvironmentMeasurement2D meas;	
	float grid_size = 90.0;
	float maxLaserDistance = 45.0;
	CBetBdtLocalizer slammer(grid_size,0.02); 
	float x=0,y=0,a=0;
	int cnt = 0;
	Grid::TScan scan;
	scan.alloc(600);
		
	//COccuGUI sgui(grid_size,0.06,1.0); ///< for visualizing the SLAM map
	float mx=0,my=0,ma=0;
	
		FILE *fmeas = fopen(scan_meas,"rt");
	//FILE *fmeas = fopen("j2b2_measurement_labrat.txt","rt");
	if(fmeas == NULL){
		fprintf(stderr,"Unable to open the measurement file '%s'!!!\n",scan_meas);
		exit(1);
	}
		
	FILE *fenv_meas = fopen(env_out_file,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
		
	FILE *fmeas_out = fopen(scan_meas_out,"wt");
	if(fenv_meas==NULL){
		fprintf(stderr,"Unable to open the log file\n");
		exit(1);
	}
	
	float search_x = .5;
	float search_y = .5;
	float search_a = 20.0*M_PI/180.0;
	
	CMatchStorage matches(100);
	bool run = true;
	
	mcl::pose init_est,diff,pos;
	mcl::pose last,current;
		
	mcl::pose reference_pose, pose_now;
	
	std::vector< std::vector<float> > scans; ///All scans for previous map
	std::vector< mcl::pose > poses;    ///All poses for previous map
	std::vector<float> tmp_scan;
		
		///Read first measurement and put it into the map
	//for(int i = 0;i<200;i++) 
	readScanLine(fmeas,scan,last);
		
	//last.set(x,y,a);
		
	slammer.setMeasurementToMap(scan, 0, 0,  0, 0.99);
	writeScanLine(fmeas_out,scan ,0.f,0.f,0.f);
		
	fprintf(stderr,"Scan has %d points\n",scan.N);
	for(int i=0;i<scan.N;i++){
		fprintf(stderr,"'%.2f %.2f' ",scan.r[i],scan.a[i]);
	}
	fprintf(stderr,"\n\n");
	slammer.tuneParameters(15.0,0.12);
	bool updated = false;
		
	while(run){
		for(int i = 0;i<1;i++) readScanLine(fmeas,scan,current);
		//current.set(x,y,a);
				
				///Save scans and poses 
		tmp_scan.clear();
		for(int i=0;i<scan.N;i++){
			tmp_scan.push_back(scan.r[i]);
		}
		scans.push_back(tmp_scan);
				
				
		diff.setToDifferentialPose(current,last);
		#warning "REMOVE THIS LATER"
		diff.set(0,0,0);
		init_est = init_est.integrateDifferential(diff);
		last=current;
				
		///Put scan to env meas class
		TMeasurementPoint p;
		meas.points.clear();
		for(int i=0;i<scan.N;i++){
			if(scan.r[i]<maxLaserDistance){
				p.x = scan.r[i]*cos(scan.a[i]);
				p.y = scan.r[i]*sin(scan.a[i]);
				meas.addPoint(p);
			}else{
				scan.r[i] = 0;
			}
		}
		meas.frame.setFrame(init_est.x,init_est.y,init_est.a);
		meas.frame.setError(search_x,search_y,search_a);
		
		timer(true);
						
		if(slammer.localize(meas,matches)){
			matches.sort();
			meas.frame.x_ref = matches.storage[0].x; meas.frame.y_ref = matches.storage[0].y; meas.frame.a_ref = matches.storage[0].a;
			init_est.set(matches.storage[0].x,matches.storage[0].y,matches.storage[0].a);
						
			pose_now = reference_pose.integrateDifferential(init_est); ///< Current pose
			writeScanLine(fmeas_out,scan, pose_now.x,pose_now.y,pose_now.a);
			fprintf(stderr,"Pose now (%.2f %.2f %.2f) ",pose_now.x,pose_now.y,pose_now.a );
			poses.push_back(init_est); ///Save pose for this scan 
						
			slammer.setMeasurementToMap(scan,init_est.x,init_est.y,init_est.a,0.99);
			//sgui.setOccupancyGrid(slammer.grid);
			/*		
			if(run && updated){
				run = sgui.plotMatchResults(matches,matches.storage[0].x,
																		matches.storage[0].y,matches.storage[0].a);
				updated = false;
			}*/
		}else{
			fprintf(stderr,"WTF!!!!\n");
		}
				
				
		////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
		////////////////////////////////////////////////////////////////////////////
		pos = init_est;
		float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
		float toEdge;
		toEdge = (grid_size/2.0)-toPos;			
		if(toEdge < 35.0 || feof(fmeas)){
			meas.setOccupancyGrid(slammer.grid, 0.8); ///< set grid to 2D measurement 
			meas.frame.setFrame(pos.x,pos.y,pos.a);		
			meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
						
			fprintf(stderr,"Saving Original map\n");
			meas.fprint(fenv_meas);        ///< Save to file
			fflush(fenv_meas);             ///< flush
			
			slammer.grid->setGridToValue(0.5); ///< Clear map
			
			reference_pose = reference_pose.integrateDifferential(init_est); ///< Current pose
			pos.set(0,0,0); ///< initialize pose
			init_est.set(0,0,0);
			
			for(unsigned int i=1;i<scans.size()-2;i+=1){
				for(int j=0;j<scan.N;j++){
					scan.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
					if(scan.r[j]>maxLaserDistance) scan.r[j]=0; 
				}
				diff.setToDifferentialPose(poses[scans.size()-1-i],poses[scans.size()-1]);
				slammer.setMeasurementToMap(scan,diff.x,diff.y,diff.a,0.99,false);
			}
			slammer.updateDistTransform();
	
			scans.clear();
			poses.clear();
		}
		timer(false);
		cnt++;	
	}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////

int main(int argc, char *argv[]){
	  int demo = 0;
		srand(time(NULL));
		const char *scan_meas=DEFAULT_J2B2_MEAS_FILE;
		const char *env_out_file = "environment_measurement2D_out.txt";
		const char *env_file = "environment_measurement2D.txt";
		const char *scan_meas_out = "bbdt_measurement.txt";		
		
		printUsage();
		if(argc < 2){
				printUsage();
				return 0;
		}
		
		
		if(argc >= 2) scan_meas = argv[1];
		if(argc >= 3) env_out_file = argv[2];
		if(argc >= 4) scan_meas_out = argv[3];
				
		ScanMatchDemo(scan_meas ,env_out_file ,scan_meas_out , false);	
										
		
	
	fprintf(stderr,"Exited\n");	
	return 0;
}